from gopigo import *	#Has the basic functions for controlling the GoPiGo Robot
import sys	#Used for closing the running program
from threading import Thread, Event
import time, random, logging

logging.basicConfig(level=logging.DEBUG,
                    format='(%(threadName)-10s) %(message)s',
                    )
					
class DancingClass(Thread):
	def __init__(self, e, t):
		Thread.__init__(self)
		self.event = e
		self.timeout = t
		self.delaytime = random.uniform(0, .5)
		self.number_of_moves = random.randint(1, 10)
		self.moves_list  = [left, right,fwd,bwd]
		self.swerve_moves_list  = [left, right]
		
	def run(self):
		self.dance()
		
	def dance(self):
		logging.debug('Delay: %f' % self.delaytime)
		logging.debug('number_of_moves: %i' % self.number_of_moves)
		while(self.number_of_moves > 0):
			logging.debug('Dancing...')
			random.choice(self.moves_list)()
			time.sleep(self.delaytime)
			self.number_of_moves = self.number_of_moves - 1		
			event_is_set = self.event.wait(self.timeout)
			if event_is_set:
				#self.swerve_robot()
				self.stop_robot()
				self.event.clear()
		logging.debug('I am tired. No more dancing')

	def swerve_robot(self):
		logging.debug('Got it! I shall swerve and dodge this object in front of us.')
		stop()
		time.sleep(.1) # without sleep robot sometimes freaks out
		random.choice(self.swerve_moves_list)()
		increase_speed()
		time.sleep(1)
		decrease_speed()

		
	def stop_robot(self):
		logging.debug('Got it! I shall stop and wait for this object to move out of our way.')
		stop()
		time.sleep(3)
		


			
if __name__ == '__main__':
	stop()
	e = Event()
	# Declare objects of MyThread class
	myThreadOb1 = DancingClass(e, 2)
	myThreadOb1.setName('DancingThr')

	# Start running the thread!
	myThreadOb1.start()
	

	distance_to_stop=10

	while myThreadOb1.number_of_moves > 0:
		dist=us_dist(15)			#Find the distance of the object in front
		logging.debug('Dist: %icm' % dist)

		#If the object is closer than the "distance_to_stop" distance, stop the GoPiGo
		if (dist<distance_to_stop) or (not myThreadOb1.isAlive()):	
			if not myThreadOb1.isAlive():
				break
			logging.debug('Something is in front of us - Help!!!!')
			#Stop the GoPiGo
			e.set()
			time.sleep(.5)
			dist=us_dist(15) #Find the distance of the object in front

		time.sleep(.5)
	
	# Wait for the threads to finish...
	myThreadOb1.join()


	stop()